#include "cplane3d.h"

/**
 * CPlane3D - creates plane identical with xz plane
 */
template <typename T>
CPlane3D<T>::CPlane3D() {
  CVector3D<T> tmpNormal(0, 1, 0);
  set(tmpNormal, 0);
}
//---------------------------------------------------------------------------

/**
 * CPlane3D - copy constructor (does deep copy)
 * @param plane
 */
template <typename T>
CPlane3D<T>::CPlane3D(const CPlane3D<T> &plane) {
  this->copyFrom(plane);
}
//---------------------------------------------------------------------------

/**
 * CPlane3D - creates plane from given normal vector and dist. from origin
 * @param normal normal vector
 * @param originDist distance from origin
 */
template <typename T>
CPlane3D<T>::CPlane3D(const CVector3D<T> &normal, T originDist) {
  set(normal, originDist);
}
//---------------------------------------------------------------------------

/**
 * CPlane3D - creates plane from given normal vector coords and origin dist.
 * @param normalX
 * @param normalY
 * @param normalZ
 * @param originDist distance from origin
 */
template <typename T>
CPlane3D<T>::CPlane3D(T normalX, T normalY, T normalZ, T originDist) {
  CVector3D<T> tmpNormal(normalX, normalY, normalZ);
  set(tmpNormal, originDist);
}
//---------------------------------------------------------------------------

/**
 * CPlane3D - creates plane from three given points
 * @param p1 first point
 * @param p2 second point
 * @param p3 third point
 */
template <typename T>
CPlane3D<T>::CPlane3D(const CVector3D<T> &p1, const CVector3D<T> &p2,
                      const CVector3D<T> &p3) {

  // Find normal vector and distance from origin
  CVector3D<T> tmpNormal(CVector3D<T>::cross(p2 - p1, p3 - p1));
  tmpNormal.normalize();
  set(tmpNormal, CVector3D<T>::dot(p1, tmpNormal));
}
//---------------------------------------------------------------------------

/**
 * operator== - compares two planes
 * @param plane
 * @return true if planes are identical
 */
template <typename T>
bool CPlane3D<T>::operator == (const CPlane3D<T> &plane) const {

  // Normal vectors coordinates must be in same ratio
  CVector3D<T> n1(this->normal), n2(plane.getNormal());
  T t(originDist / plane.getOriginDist());
  return CMath<T>::equals(n1.get(0) / n2.get(0), t) &&
         CMath<T>::equals(n1.get(1) / n2.get(1), t) &&
         CMath<T>::equals(n1.get(2) / n2.get(2), t);
}
//---------------------------------------------------------------------------

/**
 * operator!= - compares two planes
 * @param plane
 * @return true if planes are identical
 */
template <typename T>
bool CPlane3D<T>::operator != (const CPlane3D<T> &plane) const {
  return !(*this == plane);
}
//---------------------------------------------------------------------------

/**
 * set - sets normal vector and distance from origin
 * @param normal normal vector
 * @param originDist distance from origin
 */
template <typename T>
void CPlane3D<T>::set(const CVector3D<T> &normal, T originDist) {
  this->normal = normal;
  this->originDist = originDist;
  this->normal.normalize();
}
//---------------------------------------------------------------------------

/**
 * getNormal - returns normal vector
 * @return plane normal vector
 */
template <typename T>
CVector3D<T> CPlane3D<T>::getNormal() const {
  return normal;
}
//---------------------------------------------------------------------------

/**
 * getOriginDist - returns distance of plane from origin
 * @return distance from origin
 */
template <typename T>
T CPlane3D<T>::getOriginDist() const {
  return originDist;
}
//---------------------------------------------------------------------------

/**
 * pointDist - calculates distance of given point from plane
 * @param p point
 * @return distance of point from plane
 */
template <typename T>
T CPlane3D<T>::pointDist(const CVector3D<T> &p) {
  return (CVector3D<T>::dot(p, normal) + originDist) / normal.getNorm();
}
//---------------------------------------------------------------------------

/**
 * planeDist - calculates distance between two planes
 * @param plane
 * @return distance
 */
template <typename T>
T CPlane3D<T>::planeDist(const CPlane3D<T> &plane) {

  // If planes are intersecting, distance is zero
  if (isIntersecting(plane)) return 0;

  // Subtract origin distances of both planes
  return fabs(plane.getOriginDist() / plane.getNormal().getNorm() -
              originDist / normal.getNorm());
}
//---------------------------------------------------------------------------

/**
 * lineDist - calculates distance of given line from plane
 * @param l
 * @return distance of line from plane
 */
template <typename T>
T CPlane3D<T>::lineDist(const CLine3D<T> &l) {

  // If line is intersecting plane, distance is zero
  if (isIntersecting(l)) return 0;

  // Distance of arbitrary point of line from plane
  return pointDist(l.getPoint());
}

/**
 * isPointOnPlane - checks whether given point lies on plane
 * @param p point
 * @return true if point lies on plane
 */
template <typename T>
bool CPlane3D<T>::isPointOnPlane(const CVector3D<T> &p) {
  return CMath<T>::equals(CVector3D<T>::dot(p, normal) + originDist, 0) ?
         true : false;
}
//---------------------------------------------------------------------------

/**
 * isIntersecting - checks whether given plane intersects with this plane
 * @param plane
 * @return true if planes are intersecting
 */
template <typename T>
bool CPlane3D<T>::isIntersecting(const CPlane3D<T> &plane) {

  // If planes are not parallel in 3D space, they are intersecting
  return !isParallel(plane);
}
//---------------------------------------------------------------------------

/**
 * isIntersecting - checks whether given line intersects with this plane
 * @param l line
 * @return true if line and plane are intersecting
 */
template <typename T>
bool CPlane3D<T>::isIntersecting(const CLine3D<T> &l) {

  // If plane is not parallel with line in 3D, they are intersecting
  return !isParallel(l);
}
//---------------------------------------------------------------------------

/**
 * isIntersecting - checks whether line segment intersects with this plane
 * @param p1 fist edge of line segment
 * @param p2 second edge of line segment
 * @return true if line segment intersects with plane
 */
template <typename T>
bool CPlane3D<T>::isIntersecting(const CVector3D<T> &p1,
                                 const CVector3D<T> &p2) {

  // Distances of segment's edges form plane
  T dist1(CVector3D<T>::dot(normal, p1) + originDist);
  T dist2(CVector3D<T>::dot(normal, p2) + originDist);

  // If both distances has same sign, both edges lies in same half-space =>
  // line segment does not intersect with plane
  if (dist1 * dist2 >= 0) return false;
  return true;
}
//---------------------------------------------------------------------------

/**
 * isParallel - checks whether this plane is parallel with given plane
 * @param plane
 * @return true if planes are parallel
 */
template <typename T>
bool CPlane3D<T>::isParallel(const CPlane3D<T> &plane) {

  // Calculate ratio of first corresponding coordinates of normal vectors
  T t(plane.getNormal().get(0) / normal.get(0));

  // If all corresponding coordinates are in same ratio => planes are parallel
  if (CMath<T>::equals(t, plane.getNormal().get(1) / normal.get(1)) &&
      CMath<T>::equals(t, plane.getNormal().get(2) / normal.get(2))) {
    return true;
  }
  return false;
}
//---------------------------------------------------------------------------

/**
 * isParallel - checks whether this plane is parallel with given line in 3D
 * @param l line
 * @return true if line is parallel with plane
 */
template <typename T>
bool CPlane3D<T>::isParallel(const CLine3D<T> &l) {

  // If direction of line is perpendicular to plane normal => parallel
  return CMath<T>::equals(CVector3D<T>::dot(normal, l.getDirection()), 0) ?
         true : false;
}
//---------------------------------------------------------------------------

/**
 * findIntersection - finds intersection of this plane and given line
 * @param l line
 * @return intersection point
 */
template <typename T>
CVector3D<T> CPlane3D<T>::findIntersection(const CLine3D<T> &l) {

  // Denominator from intersection formulas
  T denom(CVector3D<T>::dot(normal, l.getDirection()));

  // If denominator is zero => intersection doesn't exist, exception
  if (CMath<T>::equals(denom, 0)) {
    throw CIntersectNotExistException(getClassName(), "findIntersection");
  }

  // Calculate (t = -(dot(n,a) + d) / dot(n,v)) and substitute "t" to line
  // parametric equation (p = a + vt)
  T t(-(CVector3D<T>::dot(normal, l.getPoint()) + originDist) / denom);
  return l.getPoint() + l.getDirection() * t;
}
//---------------------------------------------------------------------------

/**
 * copyFrom - replace this plane with copy of given plane
 * @param plane
 */
template <typename T>
void CPlane3D<T>::copyFrom(const CPlane3D<T> &plane) {
  this->originDist = plane.getOriginDist();
  this->normal = plane.getNormal();
}
//---------------------------------------------------------------------------

template class CPlane3D<float>;
template class CPlane3D<double>;
